#!/usr/bin/env python
"""
Supply browser view of ros topics in local network
"""

import rospy
from std_msgs.msg import Float32, Float64, Int16, String, Bool
from sensor_msgs.msg import NavSatFix
from rosgraph_msgs.msg import Log

import json
import os
import tornado.ioloop
import tornado.log
import tornado.web
import tornado.websocket

PORT = 8448
STATIC_PATH = os.path.expanduser('~/sailing-robot/dashboard/src')

simple_topics = [
    ('/heading', Float32),
    ('/heading_comp', Float32),
    # ('/pitch', Float32),
    # ('/roll', Float32),
    ('/goal_heading', Float32),
    ('/dbg_goal_wind_angle', Float32),
    ('/rudder_control', Int16),
    ('/sailing_state', String),
    ('/dbg_helming_procedure', String),
    ('/wind_direction_apparent', Float64),
    ('/dbg_distance_to_waypoint', Float32),
    ('/dbg_heading_to_waypoint', Float32),
    ('/gps_satellites', Int16),
    # ('/tack_rudder', Float32),
    ('/task_ix', Int16),
    ('/active_task_kind', String),
    ('/dbg_latest_waypoint_id', String),
    ('/wind_direction_average', Float32),
    ('/camera_detection', String),
    ('/remote_control', Bool),
]

geo_topics = [
    # '/position',
]

class MessageForwarder(object):
    """Forward ROS messages to a websocket"""
    def __init__(self):
        self.sockets = []
        self.loop = tornado.ioloop.IOLoop.current()
        for topic, rostype in simple_topics:
            self.simple_forwarding(topic, rostype)
        for topic in geo_topics:
            self.geo_forwarding(topic)
        self.rosout_forwarding()
    
    def broadcast(self, content):
        for s in self.sockets:
            s.send_json_message(content)
    
    # The callbacks we give to ROS are called on separate threads, so we use
    # loop.add_callback to ensure we actually send the data from the main thread.
    def simple_forwarding(self, topic_name, rostype):
        def forward(msg):
            self.broadcast({'topic': topic_name, 'value': msg.data})
        return rospy.Subscriber(topic_name, rostype,
                            lambda m: self.loop.add_callback(forward, m))

    def geo_forwarding(self, topic_name):
        def forward(msg):
            self.broadcast({'topic': topic_name, 'latitude': msg.latitude,
                            'longitude': msg.longitude})
        return rospy.Subscriber(topic_name, NavSatFix,
                            lambda m: self.loop.add_callback(forward, m))

    def rosout_forwarding(self):
        def forward(msg):
            d = {'topic': '/rosout', 'level': msg.level, 'name': msg.name,
              'msg': msg.msg, 'file': msg.file, 'line': msg.line,
              'function': msg.function, 'topics': msg.topics}
            self.broadcast(d)
        return rospy.Subscriber('rosout_agg', Log,
                                lambda m: self.loop.add_callback(forward, m))

class UpdateHandler(tornado.websocket.WebSocketHandler):
    def initialize(self, forwarder):
        self.forwarder = forwarder
    
    def open(self):
        self.forwarder.sockets.append(self)
    
    def on_close(self):
        self.forwarder.sockets.remove(self)
    
    def send_json_message(self, content):
        json_msg = json.dumps(content)
        self.write_message(json_msg)

def dashboard_server():
    tornado.log.enable_pretty_logging()
    forwarder = MessageForwarder()
    app = tornado.web.Application([
        (r"/updates", UpdateHandler,
            {"forwarder": forwarder}),
        (r"/(.*)", tornado.web.StaticFileHandler,
            {"path": STATIC_PATH, "default_filename": "index.html"}),
        
    ],
    compiled_template_cache=False,
    )
    app.listen(PORT, address='0.0.0.0')
    loop = tornado.ioloop.IOLoop.current()
    rospy.client.on_shutdown(loop.stop)
    loop.start()


if __name__ == '__main__':
    try:
        rospy.init_node("debugging_dashboard", anonymous=True)
        dashboard_server()
    except rospy.ROSInterruptException:
        pass
